#ifndef CAN_DRIVER_NODE_H
#define CAN_DRIVER_NODE_H

#include "ros/ros.h"
#include "can_msgs/Clamp_TX2_LevelSensor.h"
#include "can_msgs/TX2_VCU_ClampLiftCmd.h"
#include "can_msgs/TX2_VCU_ControlCmd.h"
#include "can_msgs/TX2_VCU_MotorCmd.h"
#include "can_msgs/VCU_TX2_ClampLiftStatus.h"
#include "can_msgs/VCU_TX2_ControlStatus.h"
#include "can_msgs/VCU_TX2_Diag_AD.h"
#include "can_msgs/VCU_TX2_Diag_Motor_Ampere.h"
#include "can_msgs/VCU_TX2_Diag_Motor_Status.h"
#include "can_msgs/VCU_TX2_Diag_Status.h"
#include "can_msgs/VCU_TX2_MotorStatus.h"

#include "ThreadCANETUDP.h"


class CanDriverNode
{
public:    
    CanDriverNode(int argc, char** argv);
    ~CanDriverNode();
    static void msg_handler(CAN_FRAME* frame, void* me);
    void loop();

private:
    ThreadCANETUDP *thread_canet;
    ros::NodeHandle *m_nh;

    ros::Publisher pub_VCU_TX2_ClampLiftStatus;
    ros::Publisher pub_VCU_TX2_ControlStatus;
    ros::Publisher pub_VCU_TX2_Diag_AD;
    ros::Publisher pub_VCU_TX2_Diag_Motor_Ampere;
    ros::Publisher pub_VCU_TX2_Diag_Motor_Status;
    ros::Publisher pub_VCU_TX2_Diag_Status;
    ros::Publisher pub_VCU_TX2_MotorStatus;
    ros::Publisher pub_Clamp_TX2_LevelSensor;
    ros::Subscriber sub_TX2_VCU_ClampLiftCmd;
    ros::Subscriber sub_TX2_VCU_ControlCmd;
    ros::Subscriber sub_TX2_VCU_MotorCmd;

    void callback_TX2_VCU_ClampLiftCmd(const can_msgs::TX2_VCU_ClampLiftCmd::ConstPtr& msg);
    void callback_TX2_VCU_ControlCmd(const can_msgs::TX2_VCU_ControlCmd::ConstPtr& msg);
    void callback_TX2_VCU_MotorCmd(const can_msgs::TX2_VCU_MotorCmd::ConstPtr& msg);

    void decode_VCU_TX2_ClampLiftStatus(CAN_FRAME *frame);
    void decode_VCU_TX2_ControlStatus(CAN_FRAME *frame);
    void decode_VCU_TX2_Diag_AD(CAN_FRAME *frame);
    void decode_VCU_TX2_Diag_Motor_Ampere(CAN_FRAME *frame);
    void decode_VCU_TX2_Diag_Motor_Status(CAN_FRAME *frame);
    void decode_VCU_TX2_Diag_Status(CAN_FRAME *frame);
    void decode_VCU_TX2_MotorStatus(CAN_FRAME *frame);
    void decode_Clamp_TX2_LevelSensor(CAN_FRAME *frame);
};




#endif // CAN_DRIVER_NODE_H
